
from Brain.actuators.wheels import Wheels
from Brain.sensors.encoder import Encoder 
import time

POWER = 23 

class WheelsTest:
	STEP_INTERVAL = 2.5
	def __init__(self, motors):
		self.motors = motors 

	def run(self):
		print ' forward '
		self.motors.send_raw(POWER, POWER)
		time.sleep(self.STEP_INTERVAL)
		print ' back '
		self.motors.send_raw(-POWER, -POWER)
		time.sleep(self.STEP_INTERVAL)
		print ' left '
		self.motors.send_raw(-POWER, POWER)
		time.sleep(self.STEP_INTERVAL)
		print ' right '
		self.motors.send_raw(POWER, -POWER)
		time.sleep(self.STEP_INTERVAL)
		
		self.motors.stop()


if __name__ == '__main__':
	if 0:
		test = Wheels()
		encoder = Encoder()
		encoder.setDaemon(True)
		encoder.start()
		test.write(0.1, 0.1)
		k = raw_input('aaaaaaaa ??')
		test.write(0.0, 0.0)
	else:
		motors = Wheels()
		test = WheelsTest(motors)
		test.run()
	
	print 'wheel test done.'

